#include "vsf.h"

#if defined(BCMWIFI_SDIO) || defined(BCMWIFI_SPI)

// bcm head structures and enums
enum bcm_sdpcm_headtype_t
{
	BCM_SDPCM_DATAHEAD		= 2,
	BCM_SDPCM_ASYNCHEAD		= 1,
	BCM_SDPCM_CTRLHEAD		= 0,
};
PACKED_HEAD struct PACKED_MID bcm_sdpcm_swhead_t
{
	uint8_t sequence;
	uint8_t channel_flags;
	uint8_t nextlen;
	uint8_t headlen;
	uint8_t flowcontrol;
	uint8_t credit;
	uint8_t reserved[2];
}; PACKED_TAIL
PACKED_HEAD struct PACKED_MID bcm_sdpcm_cdchead_t
{
	uint32_t cmd;
	uint32_t len;
	uint32_t flags;
#define BCM_SDPCM_CDCF_ERR			0x00000001
#define BCM_SDPCM_CDCF_IFMSK		0x0000F000
#define BCM_SDPCM_CDCF_IFSFT		12
#define BCM_SDPCM_CDCF_IDMSK		0xFFFF0000
#define BCM_SDPCM_CDCF_IDSFT		16
	uint32_t status;
}; PACKED_TAIL
PACKED_HEAD struct PACKED_MID bcm_sdpcm_bdchead_t
{
	uint8_t flags;
	uint8_t priority;
	uint8_t flags2;
	uint8_t offset;
}; PACKED_TAIL
PACKED_HEAD struct PACKED_MID bcm_sdpcm_head_t
{
	uint16_t frametag[2];
	struct bcm_sdpcm_swhead_t swhead;
}; PACKED_TAIL
PACKED_HEAD struct PACKED_MID bcm_sdpcm_ctrlpkg_t
{
	struct bcm_sdpcm_head_t common;
	struct bcm_sdpcm_cdchead_t cdchead;
}; PACKED_TAIL
PACKED_HEAD struct PACKED_MID bcm_sdpcm_datapkg_t
{
	struct bcm_sdpcm_head_t common;
	uint8_t padding[2];
	struct bcm_sdpcm_bdchead_t bdchead;
}; PACKED_TAIL

enum bcm_sdpcm_EVT_t
{
	BCMSDPCM_EVT_WAKEUP = VSFSM_EVT_USER_LOCAL + 0,
	BCMSDPCM_EVT_IOCTRL_AVAIL = VSFSM_EVT_USER_LOCAL + 1,
	BCMSDPCM_EVT_INTERRUPT = VSFSM_EVT_USER_LOCAL + 2,
	BCMSDPCM_EVT_CREDIT = VSFSM_EVT_USER_LOCAL + 3,
	
	BCMSDPCM_EVT_IOCTRL_READY = VSFSM_EVT_USER_LOCAL_INSTANT + 0,
	BCMSDPCM_EVT_IOCTRL_TO = VSFSM_EVT_USER_LOCAL_INSTANT + 1,
};

uint8_t* bcm_sdpcm_get_ioctrl_buffer(struct vsfip_buffer_t **buf, uint16_t len)
{
	if (buf != NULL)
	{
		// bcm_sdpcm_ctrlpkg_t, data
		uint32_t headlen = sizeof(struct bcm_sdpcm_ctrlpkg_t);
		struct vsfip_buffer_t *buffer = VSFIP_BUF_GET(headlen + len);
		
		if (buffer != NULL)
		{
			buffer->buf.buffer += headlen;
			buffer->buf.size -= headlen;
			*buf = buffer;
			return buffer->buf.buffer;
		}
	}
	return NULL;
}

uint8_t* bcm_sdpcm_get_iovar_buffer(struct vsfip_buffer_t **buf,
									uint16_t len, const char *name)
{
	if (buf != NULL)
	{
		struct vsfip_buffer_t *buffer;
		uint8_t *bufptr =
				bcm_sdpcm_get_ioctrl_buffer(&buffer, len + strlen(name) + 1);
		
		if (buffer != NULL)
		{
			strcpy((char *)buffer->buf.buffer, name);
			buffer->buf.buffer += strlen(name) + 1;
			buffer->buf.size -= strlen(name) + 1;
			*buf = buffer;
			return buffer->buf.buffer;
		}
	}
	return NULL;
}

static void bcm_sdpcm_bus_credit_update(struct bcm_sdpcm_t *sdpcm, uint8_t *buf)
{
	struct bcm_sdpcm_head_t *head = (struct bcm_sdpcm_head_t *)buf;
	if ((head->swhead.channel_flags & 0x0F) < 3)
	{
		sdpcm->credit = head->swhead.credit;
	}
	sdpcm->flowcontrol = head->swhead.flowcontrol;
	
	if (!sdpcm->flowcontrol && (sdpcm->credit != sdpcm->sequence) &&
		sdpcm->out_pending_for_credit)
	{
		sdpcm->out_pending_for_credit = false;
		vsfsm_post_evt(&sdpcm->outthread_sm, BCMSDPCM_EVT_CREDIT);
	}
}

static void bcm_sdpcm_input(struct bcm_wifi_t *wifi, struct vsfip_buffer_t *buf)
{
	struct bcm_sdpcm_t *sdpcm = &wifi->sdpcm;
	struct bcm_sdpcm_head_t *head = (struct bcm_sdpcm_head_t *)buf->buf.buffer;
	uint16_t size = head->frametag[0];
	uint8_t *sdpcm_payload;
	
	// check size and type
	if ((size != (uint16_t)~head->frametag[1]) || (size < 12))
	{
		vsfip_buffer_release(buf);
		return;
	}
	
	bcm_sdpcm_bus_credit_update(&wifi->sdpcm, (uint8_t *)head);
	if (12 == size)
	{
		vsfip_buffer_release(buf);
		return;
	}
	sdpcm_payload = (uint8_t*)head + head->swhead.headlen;
	
	switch (head->swhead.channel_flags & 0x0F)
	{
	case BCM_SDPCM_CTRLHEAD:
	{
		struct bcm_sdpcm_cdchead_t *cdchead;
		uint16_t ioctrl_id;
		
		if (size < (sizeof(head->frametag) +
					sizeof(struct bcm_sdpcm_swhead_t) +
					sizeof(struct bcm_sdpcm_cdchead_t)))
		{
			vsfip_buffer_release(buf);
			break;
		}
		
		cdchead = (struct bcm_sdpcm_cdchead_t *)sdpcm_payload;
		ioctrl_id = (uint16_t)\
			((LE_TO_SYS_U32(cdchead->flags) & BCM_SDPCM_CDCF_IDMSK) >>
			 BCM_SDPCM_CDCF_IDSFT);
		
		// check if the reply is what we are pending for
		if (ioctrl_id == sdpcm->ioctrl_id)
		{
			sdpcm->ioctrl_reply = buf;
			vsfsm_sem_post(&sdpcm->ioctrl_sem);
		}
		else
		{
			vsfip_buffer_release(buf);
		}
	}
		break;
	case BCM_SDPCM_DATAHEAD:
	{
		struct bcm_sdpcm_bdchead_t *bdchead;
		uint32_t payload_offset;
		
		if (size < (sizeof(struct bcm_sdpcm_bdchead_t) +
					sizeof(struct bcm_sdpcm_head_t)))
		{
			vsfip_buffer_release(buf);
			break;
		}
		
		bdchead = (struct bcm_sdpcm_bdchead_t *)sdpcm_payload;
		payload_offset = head->swhead.headlen +
			sizeof(struct bcm_sdpcm_bdchead_t) + (bdchead->offset << 2);
		buf->buf.buffer += payload_offset;
		buf->buf.size -= payload_offset;
		
		buf->netif = &wifi->netif;
		vsfip_eth_input(buf);
	}
		break;
	case BCM_SDPCM_ASYNCHEAD:
	{
		struct bcm_sdpcm_bdchead_t* bdchead;
		struct bcm_sdpcm_evtpkg_t* evtpkg;
		struct bcm_sdpcm_evthead_t *evthead;
		uint32_t payload_offset;
		
		bdchead = (struct bcm_sdpcm_bdchead_t *)sdpcm_payload;
		payload_offset = sizeof(struct bcm_sdpcm_bdchead_t) +
							(bdchead->offset << 2);
		evtpkg = (struct bcm_sdpcm_evtpkg_t *)\
								((uint8_t *)bdchead + payload_offset);
		payload_offset += head->swhead.headlen +
			sizeof(struct bcm_ethhead_t) + sizeof(struct bcm_sdpcm_ethhead_t);
		buf->buf.buffer += payload_offset;
		buf->buf.size -= payload_offset;
		
		if ((BE_TO_SYS_U16(evtpkg->ethhead.ethertype) != BCM_ETHTYPE) ||
			memcmp(evtpkg->sdpcm_ethhead.oui, BCM_OUI, 3))
		{
			vsfip_buffer_release(buf);
			break;
		}
		
		evthead = &evtpkg->evthead;
		evthead->flags = BE_TO_SYS_U16(evthead->flags);
		evthead->evttype = BE_TO_SYS_U32(evthead->evttype);
		evthead->status = BE_TO_SYS_U32(evthead->status);
		evthead->reason = BE_TO_SYS_U32(evthead->reason);
		evthead->authtype = BE_TO_SYS_U32(evthead->authtype);
		evthead->datalen = BE_TO_SYS_U32(evthead->datalen);
		evthead->ifidx = evthead->ifidx & 3;
		
		if (sdpcm->async_pending)
		{
			if (sdpcm->async_evt != NULL)
			{
				// TODO: what to do if previous event not processed
				// ignore now
			}
			sdpcm->async_evt = buf;
			vsfsm_sem_post(&sdpcm->async_sem);
		}
		else
		{
			vsfip_buffer_release(buf);
		}
	}
		break;
	default:
		vsfip_buffer_release(buf);
	}
}

void bcm_sdpcm_int(void *param)
{
	struct bcm_sdpcm_t *sdpcm = (struct bcm_sdpcm_t *)param;
	vsfsm_post_evt_pending(&sdpcm->inthread_sm, BCMSDPCM_EVT_INTERRUPT);
}

static vsf_err_t bcm_sdpcm_input_thread(struct vsfsm_pt_t *pt, vsfsm_evt_t evt)
{
	struct bcm_wifi_t *wifi = (struct bcm_wifi_t *)pt->user_data;
	struct bcm_bus_t *bus = &wifi->bus;
	struct bcm_sdpcm_t *sdpcm = &wifi->sdpcm;
	vsf_err_t err;
	
	if (evt == BCMSDPCM_EVT_INTERRUPT)
	{
		sdpcm->interrupted = true;
		vsfsm_sem_post(&sdpcm->in_sem);
		return VSFERR_NONE;
	}
	
	vsfsm_pt_begin(pt);
	
	sdpcm->intrans_pt.sm = pt->sm;
	sdpcm->intrans_pt.user_data = &wifi->bus;
	
	while (!sdpcm->quit)
	{
		bus->op->enable_int(bus);
		
		if (vsfsm_sem_pend(&sdpcm->in_sem, pt->sm))
		{
			vsfsm_pt_wfe(pt, BCMSDPCM_EVT_WAKEUP);
		}
		
		// process input, when interrupted
		while (!sdpcm->quit && (sdpcm->interrupted|| bus->op->is_int(bus)))
		{
			sdpcm->interrupted = false;
			
			sdpcm->intrans_pt.user_data = bus;
			sdpcm->intrans_pt.state = 0;
			vsfsm_pt_entry(pt);
			err = bus->op->f2_avail(&sdpcm->intrans_pt, evt, &sdpcm->f2_size);
			if (err > 0) return err; else if (err < 0) continue;
			
			if (sdpcm->f2_size > 0)
			{
				sdpcm->currxbuf = VSFIP_BUF_GET(bus->op->f2size(sdpcm->f2_size));
				if (NULL == sdpcm->currxbuf)
				{
					// can not get buffer
					// read first 12 bytes for credit and flowcontrol then abort
					sdpcm->intrans_pt.state = 0;
					sdpcm->headbuffer.buffer = sdpcm->head_buff_mem;
					sdpcm->headbuffer.size = 12;
					vsfsm_pt_entry(pt);
					err = bus->op->f2_read(&sdpcm->intrans_pt, evt,
							bus->type == BCM_BUS_TYPE_SPI ? sdpcm->f2_size : 12,
							&sdpcm->headbuffer);
					if (err > 0) return err; else if (err < 0) continue;

#ifdef BCMWIFI_SDIO
					// will also abort if previous read of 12 bytes fail
					sdpcm->intrans_pt.state = 0;
					vsfsm_pt_entry(pt);
					err = bus->op->f2_abort(&sdpcm->intrans_pt, evt);
					if (err > 0) return err;
					// ignore failure, will go on to update credit
#endif

					bcm_sdpcm_bus_credit_update(&wifi->sdpcm,
												sdpcm->headbuffer.buffer);
				}
				else
				{
					// receive f2
					sdpcm->intrans_pt.state = 0;
					vsfsm_pt_entry(pt);
					err = bus->op->f2_read(&sdpcm->intrans_pt, evt,
								sdpcm->f2_size, &sdpcm->currxbuf->buf);
					if (err > 0) return err;
					if (err < 0)
					{
						vsfip_buffer_release(sdpcm->currxbuf);
						continue;
					}
					
					bcm_sdpcm_input(wifi, sdpcm->currxbuf);
				}
			}
		}
	}
	
	vsfsm_pt_end(pt);
	return VSFERR_NONE;
}

static vsf_err_t bcm_sdpcm_output_thread(struct vsfsm_pt_t *pt, vsfsm_evt_t evt)
{
	struct bcm_wifi_t *wifi = (struct bcm_wifi_t *)pt->user_data;
	struct vsfip_netif_t *netif = &wifi->netif;
	struct bcm_sdpcm_t *sdpcm = &wifi->sdpcm;
	struct vsfq_node_t *node;
	vsf_err_t err;
	
	vsfsm_pt_begin(pt);
	
	sdpcm->outtrans_pt.sm = pt->sm;
	sdpcm->outtrans_pt.user_data = &wifi->bus;
	
	while (!sdpcm->quit)
	{
		if (vsfsm_sem_pend(&netif->output_sem, pt->sm))
		{
			vsfsm_pt_wfe(pt, VSFSM_EVT_USER_LOCAL);
		}
		
		node = vsfq_dequeue(&netif->outq);
		sdpcm->curtxbuf = container_of(node, struct vsfip_buffer_t, netif_node);
		if (sdpcm->curtxbuf != NULL)
		{
			// process output if credit available and not flowcontrol
			if (sdpcm->flowcontrol || (sdpcm->credit == sdpcm->sequence))
			{
				// wait for credit
				sdpcm->out_pending_for_credit = true;
				vsfsm_pt_wfe(pt, BCMSDPCM_EVT_CREDIT);
			}
			
			// fix sequence
			((struct bcm_sdpcm_head_t *)sdpcm->curtxbuf->buf.buffer)\
						->swhead.sequence = sdpcm->sequence++;
			
			sdpcm->outtrans_pt.state = 0;
			vsfsm_pt_entry(pt);
			err = bcm_bus_transact(&sdpcm->outtrans_pt, evt, BCM_CMD_WRITE,
									BCM_FUNC_F2, 0, sdpcm->curtxbuf->buf.size,
									&sdpcm->curtxbuf->buf);
			if (err > 0) return err;
			
			vsfip_buffer_release(sdpcm->curtxbuf);
		}
	}
	
	vsfsm_pt_end(pt);
	return VSFERR_NONE;
}

vsf_err_t bcm_sdpcm_init(struct bcm_wifi_t *wifi)
{
	struct bcm_sdpcm_t *sdpcm = &wifi->sdpcm;
	
	wifi->netif.drv->netif_header_size = sizeof(struct bcm_sdpcm_datapkg_t);
	sdpcm->credit = 0;
	sdpcm->flowcontrol = 1;
	sdpcm->sequence = 0;
	sdpcm->ioctrl_id = 0;
	sdpcm->async_pending = false;
	sdpcm->async_evt = NULL;
	sdpcm->interrupted = false;
	sdpcm->out_pending_for_credit = false;
	
	sdpcm->ioctrl_to.interval = 500;
	sdpcm->quit = false;
	vsfsm_crit_init(&sdpcm->ioctrl_crit, BCMSDPCM_EVT_IOCTRL_AVAIL);
	vsfsm_sem_init(&sdpcm->ioctrl_sem, 0, BCMSDPCM_EVT_IOCTRL_READY);
	
	wifi->bus.op->init_int(&wifi->bus, bcm_sdpcm_int, sdpcm);
	
	vsfsm_sem_init(&sdpcm->in_sem, 0, BCMSDPCM_EVT_WAKEUP);
	sdpcm->inthread_pt.thread = bcm_sdpcm_input_thread;
	sdpcm->inthread_pt.user_data = wifi;
	vsfsm_pt_init(&sdpcm->inthread_sm, &sdpcm->inthread_pt);
	
	sdpcm->outthread_pt.thread = bcm_sdpcm_output_thread;
	sdpcm->outthread_pt.user_data = wifi;
	vsfsm_pt_init(&sdpcm->outthread_sm, &sdpcm->outthread_pt);
	
	return VSFERR_NONE;
}

vsf_err_t bcm_sdpcm_fini(struct bcm_wifi_t *wifi)
{
	wifi->sdpcm.quit = true;
	wifi->bus.op->fini_int(&wifi->bus);
	return VSFERR_NONE;
}

static void bcm_sdpcm_header_common(struct vsfip_buffer_t *buf,
									enum bcm_sdpcm_headtype_t type)
{
	struct bcm_sdpcm_head_t *head = (struct bcm_sdpcm_head_t *)buf->buf.buffer;
	
	memset(head, 0, sizeof(struct bcm_sdpcm_head_t));
	head->swhead.channel_flags = (uint8_t)type;
	head->swhead.headlen = sizeof(struct bcm_sdpcm_head_t) +\
										((BCM_SDPCM_DATAHEAD == type) ? 2 : 0);
	head->swhead.sequence = 0;
	head->frametag[0] = SYS_TO_LE_U16((uint16_t)buf->buf.size);
	head->frametag[1] = SYS_TO_LE_U16((uint16_t)~buf->buf.size);
}

static vsf_err_t bcm_sdpcm_output_common(struct vsfip_netif_t *netif,
					struct vsfip_buffer_t *buf, enum bcm_sdpcm_headtype_t type)
{
	bcm_sdpcm_header_common(buf, type);
	vsfq_append(&netif->outq, &buf->netif_node);
	return vsfsm_sem_post(&netif->output_sem);
}

static vsf_err_t bcm_sdpcm_header(struct vsfip_buffer_t *buf,
			enum vsfip_netif_proto_t proto, const struct vsfip_macaddr_t *dest,
			uint32_t interface)
{
	struct bcm_sdpcm_datapkg_t *pkg;
	
	if (vsfip_eth_header(buf, proto, dest) ||
		(buf->buf.buffer - buf->buffer < sizeof(struct bcm_sdpcm_datapkg_t)))
	{
		return VSFERR_NOT_ENOUGH_RESOURCES;
	}
	
	buf->buf.buffer -= sizeof(struct bcm_sdpcm_datapkg_t);
	buf->buf.size += sizeof(struct bcm_sdpcm_datapkg_t);
	pkg = (struct bcm_sdpcm_datapkg_t *)buf->buf.buffer;
	pkg->bdchead.flags = 0x20;
	pkg->bdchead.flags2 = interface & 3;
	pkg->bdchead.priority = 0;
	pkg->bdchead.offset = 0;
	
	bcm_sdpcm_header_common(buf, BCM_SDPCM_DATAHEAD);
	return VSFERR_NONE;
}

vsf_err_t bcm_sdpcm_header_sta(struct vsfip_buffer_t *buf,
			enum vsfip_netif_proto_t proto, const struct vsfip_macaddr_t *dest)
{
	return bcm_sdpcm_header(buf, proto, dest, BCM_INTERFACE_STA);
}

vsf_err_t bcm_sdpcm_header_ap(struct vsfip_buffer_t *buf,
			enum vsfip_netif_proto_t proto, const struct vsfip_macaddr_t *dest)
{
	return bcm_sdpcm_header(buf, proto, dest, BCM_INTERFACE_AP);
}

// buf MUST be allocated by bcm_sdpcm_get_iovar_buffer or
// bcm_sdpcm_get_ioctrl_buffer
vsf_err_t bcm_sdpcm_ioctrl(struct vsfsm_pt_t *pt, vsfsm_evt_t evt,
				uint32_t interface, uint32_t type, uint32_t cmd,
				struct vsfip_buffer_t *buf, struct vsfip_buffer_t **reply)
{
	struct bcm_wifi_t *wifi = (struct bcm_wifi_t *)pt->user_data;
	struct bcm_sdpcm_t *sdpcm = &wifi->sdpcm;
	struct bcm_sdpcm_ctrlpkg_t *ctrlpkgtx, *ctrlpkgrx;
	vsf_err_t err = VSFERR_NONE;
	
	vsfsm_pt_begin(pt);
	
	// enter critical
	if (vsfsm_crit_enter(&sdpcm->ioctrl_crit, pt->sm))
	{
		vsfsm_pt_wfe(pt, BCMSDPCM_EVT_IOCTRL_AVAIL);
	}
	
	// prepare ioctrl buffer
	buf->buf.size += (buf->buf.buffer - buf->buffer);
	buf->buf.buffer = buf->buffer;
	ctrlpkgtx = (struct bcm_sdpcm_ctrlpkg_t *)buf->buf.buffer;
	ctrlpkgtx->cdchead.cmd = SYS_TO_LE_U32(cmd);
	ctrlpkgtx->cdchead.len = SYS_TO_LE_U32(
							buf->buf.size - sizeof(struct bcm_sdpcm_ctrlpkg_t));
	ctrlpkgtx->cdchead.flags = SYS_TO_LE_U32(
		((++sdpcm->ioctrl_id << BCM_SDPCM_CDCF_IDSFT) & BCM_SDPCM_CDCF_IDMSK) |
		type | (interface << 12));
	ctrlpkgtx->cdchead.status = SYS_TO_LE_U32(0);
	
	// link to sendqueue
	bcm_sdpcm_output_common(&wifi->netif, buf, BCM_SDPCM_CTRLHEAD);
	
	// wait reply
	if (vsfsm_sem_pend(&sdpcm->ioctrl_sem, pt->sm))
	{
		vsfsm_notifier_set_evt(&sdpcm->ioctrl_to.notifier, pt->sm, BCMSDPCM_EVT_IOCTRL_TO);
		sdpcm->ioctrl_to.trigger_cnt = 1;
		vsftimer_enqueue(&sdpcm->ioctrl_to);
		
		evt = VSFSM_EVT_INVALID;
		vsfsm_pt_entry(pt);
		if ((evt != BCMSDPCM_EVT_IOCTRL_TO) &&
			(evt != BCMSDPCM_EVT_IOCTRL_READY))
		{
			return VSFERR_NOT_READY;
		}
		vsftimer_dequeue(&sdpcm->ioctrl_to);
		if (BCMSDPCM_EVT_IOCTRL_TO == evt)
		{
			vsfsm_sync_cancel(&sdpcm->ioctrl_sem, pt->sm);
			err = VSFERR_FAIL;
			goto end;
		}
		else if (BCMSDPCM_EVT_IOCTRL_READY == evt)
		{
			// ready
		}
	}
	
	// process reply
	if (sdpcm->ioctrl_reply != NULL)
	{
		ctrlpkgrx =
			(struct bcm_sdpcm_ctrlpkg_t *)sdpcm->ioctrl_reply->buf.buffer;
		if (ctrlpkgrx != NULL)
		{
			if (LE_TO_SYS_U32(ctrlpkgrx->cdchead.flags) & BCM_SDPCM_CDCF_ERR)
			{
				vsfip_buffer_release(sdpcm->ioctrl_reply);
				sdpcm->ioctrl_reply = NULL;
				if (reply)
				{
					*reply = NULL;
				}
				err = VSFERR_FAIL;
				goto end;
			}
		}
	}
	if (reply != NULL)
	{
		sdpcm->ioctrl_reply->buf.buffer += sizeof(struct bcm_sdpcm_ctrlpkg_t);
		sdpcm->ioctrl_reply->buf.size -= sizeof(struct bcm_sdpcm_ctrlpkg_t);
		*reply = sdpcm->ioctrl_reply;
	}
	else if (sdpcm->ioctrl_reply != NULL)
	{
		vsfip_buffer_release(sdpcm->ioctrl_reply);
	}
	sdpcm->ioctrl_reply = NULL;
	
end:
	vsfsm_crit_leave(&sdpcm->ioctrl_crit);
	vsfsm_pt_end(pt);
	return err;
}

#endif
